package trackClustering.example.spatial;

import trackClustering.example.entity.TrajectoryPoint;

import java.util.*;

/**
 * @author dll
 */
public class TrajectoryProcessorByThreshold {

    public static Map<Integer, List<TrajectoryPoint>> groupTrajectories(List<TrajectoryPoint> points) {
        Map<Integer, List<TrajectoryPoint>> trajectories = new HashMap<>();
        int trajectoryId = 0;

        for (int i = 0; i < points.size(); i++) {
            TrajectoryPoint currentPoint = points.get(i);

            if (i == 0 || !isSameTrajectory(points.get(i - 1), currentPoint)) {
                trajectoryId++;
                trajectories.putIfAbsent(trajectoryId, new ArrayList<>());
            }

            trajectories.get(trajectoryId).add(currentPoint);
        }

        return trajectories;
    }

    private static boolean isSameTrajectory(TrajectoryPoint prevPoint, TrajectoryPoint currentPoint) {
        // Define a threshold for distance and speed to determine if points belong to the same trajectory
        double distanceThreshold = 0.03; // 距离阈值差为30m
//        double speedThreshold = 10; // 速度阈值差为10m/s

        double distance = calculateDistance(prevPoint, currentPoint);
//        double speedChange = Math.abs(prevPoint.getHorizontalSpeed() - currentPoint.getHorizontalSpeed());

        return distance <= distanceThreshold;
    }

    private static double calculateDistance(TrajectoryPoint p1, TrajectoryPoint p2) {
        double lat1 = Math.toRadians(p1.getLatitude());
        double lon1 = Math.toRadians(p1.getLongitude());
        double lat2 = Math.toRadians(p2.getLatitude());
        double lon2 = Math.toRadians(p2.getLongitude());

        double dLat = lat2 - lat1;
        double dLon = lon2 - lon1;

        double a = Math.pow(Math.sin(dLat / 2), 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.pow(Math.sin(dLon / 2), 2);

        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

        double radius = 6371; // Radius of Earth in kilometers
        return radius * c;
    }
}
